trusty: Run bl33 in EL1 instead of EL2 is trusty image is 32-bit

The secure physical timer is inacessible from 32-bit S-EL1 (when EL3
is 64-bit) so trusty will use the non-secure physical timer in this
case. Linux will use the virtual timer instead of the physical timer
when started in EL1.

Change-Id: Ie49348d9a27e5287676dd4a77f678ecbd6c2309f
Signed-off-by: Arve Hjønnevåg <[email protected]>

diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index 6e80b7b7238e785ae782f8b56223bf50c99752c8..7fa207a3841e64355e12ae857d6e5d4246ab1365 100644 (file)
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -448,6 +448,31 @@ static int32_t trusty_setup(void)
        if (ret)
                ERROR("trusty: failed to register fiq handler, ret = %d\n", ret);
 
+       if (aarch32) {
+               entry_point_info_t *ns_ep_info;
+               uint32_t spsr;
+
+               ns_ep_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
+               if (!ep_info) {
+                       NOTICE("Trusty: non-secure image missing.\n");
+                       return -1;
+               }
+               spsr = ns_ep_info->spsr;
+               if (GET_RW(spsr) == MODE_RW_64 && GET_EL(spsr) == MODE_EL2) {
+                       spsr &= ~(MODE_EL_MASK << MODE_EL_SHIFT);
+                       spsr |= MODE_EL1 << MODE_EL_SHIFT;
+               }
+               if (GET_RW(spsr) == MODE_RW_32 && GET_M32(spsr) == MODE32_hyp) {
+                       spsr &= ~(MODE32_MASK << MODE32_SHIFT);
+                       spsr |= MODE32_svc << MODE32_SHIFT;
+               }
+               if (spsr != ns_ep_info->spsr) {
+                       NOTICE("Trusty: Switch bl33 from EL2 to EL1 (spsr 0x%x -> 0x%x)\n",
+                              ns_ep_info->spsr, spsr);
+                       ns_ep_info->spsr = spsr;
+               }
+       }
+
        return 0;
 }